import serial
import time
import threading
import json
import paho.mqtt.client as mqtt

ultra1_send_data = b'\x10\x03\x01\x01\x00\x01\xD7\x77'
ultra2_send_data =b'\x11\x03\x01\x01\x00\x01\xD6\xA6'
io_send_data=b'\x02\x02\x00\x00\x00\x04\x79\xFA'
gps_send_data=b'\x01\x03\x00\x05\x00\x23\x14\x12'
MQTT_BROKER_ADDRESS = '127.0.0.1'  # MQTT Broker 地址
sub_topic1="/rjgf/znzb/dev00001/setting"
pub_topic1="/rjgf/znzb/dev00001/res"
client = mqtt.Client()


def on_connect(client, userdata, flags, rc):
    global sub_topic1
    if rc == 0:
        client.subscribe(sub_topic1)
        print("Connected to MQTT Broker")
    else:
        print(f"Failed to connect, return code {rc}")


def on_disconnect(client, userdata, rc):
    try:
        print("Disconnected from MQTT Broker")
        time.sleep(5)
        client.reconnect()
    except:
        print("audio_to_txt.py on_disconnect")


def on_message(client, userdata, msg):
    try:
        print(f"Received message: {msg.payload.decode()} on topic {msg.topic}")
        if msg.topic == sub_topic1:
            json_obj = json.loads(msg.payload.decode())
            print(json_obj)
    except:
        print("main.py error")


def mqtt_thread_fun():
    global client
    global MQTT_BROKER_ADDRESS
    client.on_connect = on_connect
    client.on_message = on_message
    client.connect(MQTT_BROKER_ADDRESS, 1883, 60)
    client.on_disconnect = on_disconnect
    client.loop_start()

def modbus_crc16(data: bytes) -> int:
    """
    计算Modbus RTU的CRC16校验值
    :param data: 要计算CRC的数据，类型为字节序列
    :return: CRC16校验值
    """
    crc = 0xFFFF  # CRC初始值
    for byte in data:
        crc ^= byte  # 将数据字节与CRC寄存器的低字节异或
        for _ in range(8):  # 进行8次位移
            if crc & 0x0001:  # 如果最低位为1
                crc = (crc >> 1) ^ 0xA001  # 与多项式0xA001异或
            else:
                crc >>= 1  # 否则只是右移1位
    return crc

def usert_thread_task():
    global client
    global pub_topic1
    state=0
    door_sw=0
    car_sw=0
    ultrasound1 = 10000
    ultrasound2 = 10000
    gps_data=""
    ultrasound1_error_count = 0
    ultrasound2_error_count = 0
    io_error_count = 0
    gps_error_count = 0
    

    while 1:
        time.sleep(0.1)
        try:
            if state==0:
                ser = serial.Serial('/dev/ttyUSB0', 9600, timeout=0.2)
                if ser.isOpen():
                    print("串口已打开：", ser.portstr)
                    ultrasound1_error_count = 0
                    ultrasound2_error_count = 0
                    io_error_count = 0
                    state=1
            elif state==1:
                ser.write(ultra1_send_data)
                res1=ser.read(100)
                print("ultrasound1",res1, len(res1),res1[3:5])
                # 01 03 02 02 EF F8 A8
                if len(res1) == 7:
                    temp = res1[:-2]
                    crc = modbus_crc16(temp)
                    recv_crc = int.from_bytes(res1[-2:], byteorder='little')
                    if crc == recv_crc:
                        ultrasound1 = int.from_bytes(res1[3:5], byteorder='big')
                        ultrasound1_error_count=0
                    else:
                        ultrasound1_error_count=ultrasound1_error_count+1
                else:
                    ultrasound1_error_count = ultrasound1_error_count + 1
                # 超过三次异常 设置超声波数据归0
                if ultrasound1_error_count > 3:
                    ultrasound1_error_count = 0
                    ultrasound1 = -1
                state=2
            elif state == 2:
                ser.write(ultra2_send_data)
                res1 = ser.read(100)
                print("ultrasound2",res1,len(res1),res1[3:5])
                if len(res1) == 7:
                    temp = res1[:-2]
                    crc = modbus_crc16(temp)
                    recv_crc = int.from_bytes(res1[-2:], byteorder='little')
                    if crc == recv_crc:
                        ultrasound2 = int.from_bytes(res1[3:5], byteorder='big')
                        ultrasound2_error_count = 0
                    else:
                        ultrasound2_error_count=ultrasound2_error_count+1
                else:
                    ultrasound2_error_count = ultrasound2_error_count + 1
                # 超过三次异常 设置超声波数据归0
                if ultrasound2_error_count > 3:
                    ultrasound2_error_count = 0
                    ultrasound2 = -1
                state=3
            elif state==3:
                ser.write(io_send_data)
                res1=ser.read(100)
                print("IO",res1,len(res1),res1[3:4])
                # FE 02 01 00 91 9C
                if len(res1) == 6:
                    temp = res1[:-2]
                    crc = modbus_crc16(temp)
                    recv_crc = int.from_bytes(res1[-2:], byteorder='little')
                    if crc == recv_crc:
                        temp = int.from_bytes(res1[3:4], byteorder='little')
                        if temp & 0x01 or temp & 0x02:
                            car_sw = 1
                        else:
                            car_sw=0
                        if temp & 0x04 or temp & 0x08:
                            door_sw = 1
                        else:
                            door_sw=0
                        io_error_count=0
                    else:
                        io_error_count=io_error_count+1
                else:
                    io_error_count = io_error_count + 1

                if io_error_count>3:
                    io_error_count=0
                    car_sw = -1
                    door_sw= -1
                state = 4
            elif state==4:
                ser.write(gps_send_data)
                time.sleep(0.1)
                res1=ser.read(100)
                print("GPS",res1,len(res1),res1[3:3])
                # 一共返回 0x46 + 5 字节的数据 
                # 01 03 46 24 47 4E 52 4D 43 2C 30 31 33 32 32 36 2E 32 35 2C 56 2C 30 30 30 30 2E 30 30 30 30 30 2C 4E 2C 30 30 30 30 30 2E 30 30 30 30 30 2C 45 2C 30 30 30 2E 30 30 2C 30 30 30 2E 30 30 2C 30 36 30 31 38 30 2A 33 36 00 DB E9 
                if len(res1) == 75:
                    temp = res1[:-2]
                    crc = modbus_crc16(temp)
                    recv_crc = int.from_bytes(res1[-2:], byteorder='little')
                    if crc == recv_crc:
                        temp=res1[3:-3] # python字符串不包含后面的字节0x00
                        gps=temp.decode("utf-8")
                        print("gps success",gps)
                        gps_error_count=0
                    else:
                        gps_error_count=gps_error_count+1
                else:
                    gps_error_count = gps_error_count + 1

                if gps_error_count>3:
                    gps_error_count=0
                    gps=""
                state = 5
            elif state==5:
                str1={
                    "door_sw": car_sw,
                    "car_sw": door_sw,
                    "ultrasound1": ultrasound1,
                    "ultrasound2": ultrasound2,
                    "gps": gps
                }
                json_res=json.dumps(str1)
                client.publish(pub_topic1, payload=json_res.encode())
                state = 1
            else:
                ser.close()
                state=0
        except:
            ser.close()
            print("usert_thread_task error", state)
            state=0



if __name__=="__main__":
    thread1 = threading.Thread(target=usert_thread_task)
    thread1.daemon = True
    thread1.start()
    mqtt_thread_fun()
    while 1:
        time.sleep(1)
    pass


# 测试代码
# \xFE\x02\x00\x00\x00\x04\x6D\xC6
# data = b'\xFE\x05\x00\x00\xFF\x00'
# data = b'\xff\x03\x01\x01\x00\x01'  # 示例数据
# data = b'\x11\x06\x02\x00\x00\x01'  # 示例数
# data=b'\x02\x02\x00\x00\x00\x04'
# crc_value = modbus_crc16(data)
# print(f"CRC16值: {crc_value:04X}")  # 以十六进制输出





